#include "Hchassis.h"

void Hchassis::fast_turn_enc_absolute(double rotation, double speed,
                                     int timeout_ms, Block waitForCompletion,
                                     rotationUnits units_r,
                                     velocityUnits units_v) {
  // reset_enc();//编码器清零
  p_au_r->act_for_enc_absolute(-rotation, speed, timeout_ms,  Block::no,
                               false, 0, units_r, units_v);
  p_au_l->act_for_enc_absolute(rotation, speed, timeout_ms,
                               waitForCompletion, false, 0, units_r,
                               units_v);
  wait(1);
}

void Hchassis::fast_turn_enc_relative(double rotation, double speed,
                                     int timeout_ms, Block waitForCompletion,
                                     rotationUnits units_r,
                                     velocityUnits units_v) {
  p_au_l->act_for_enc_relative(rotation, speed, timeout_ms,  Block::no,
                               false, 0, units_r, units_v);
  p_au_r->act_for_enc_relative(-rotation, speed, timeout_ms,
                               waitForCompletion, false, 0, units_r,
                               units_v);
  wait(1);
}

void Hchassis::fast_turn_dist_absolute(double dist, double speed, int timeout_ms,
                                      Block waitForCompletion,
                                      distanceUnits units_d,
                                      velocityUnits units_v) {
  if (timeout_ms < 0) {
    cout << __PRETTY_FUNCTION__ << endl;
    cout << "error: timeout<=0\n\n";
    return;
  }
  double shaft_aim_rev = distance_to_rev(dist, units_d);
  fast_turn_enc_absolute(shaft_aim_rev, speed, timeout_ms, waitForCompletion,
                         rotationUnits::rev, units_v);
}

void Hchassis::fast_turn_dist_relative(double dist, double speed, int timeout_ms,
                                      Block waitForCompletion,
                                      distanceUnits units_d,
                                      velocityUnits units_v) {
  if (timeout_ms < 0) {
    cout << __PRETTY_FUNCTION__ << endl;
    cout << "error: timeout<=0\n\n";
    return;
  }

  double shaft_aim_rev = distance_to_rev(dist, units_d);
  fast_turn_enc_relative(shaft_aim_rev, speed, timeout_ms, waitForCompletion,
                         rotationUnits::rev, units_v);
}